#coding:utf-8
# find path on 2d map
from astar import AStar
import numpy as np
from scipy.ndimage.filters import gaussian_filter
#import matplotlib.pyplot as plt
import math, pickle, configparser, argparse
from utils import compute_dis_lat_lon, get_val_on_straight_line
from energyModel import flight_energy
from config import cfg
from gen_map_from_txt import parse_txt

#map_file = 'map_%s_%s.pkl' % (cfg.from_, cfg.to_)
#lat, lon, alt, image_id, start_xy, stop_xy = pickle.load(open(map_file, 'rb'))

parser = argparse.ArgumentParser()
parser.add_argument('--config', required=True, help='configure file, for example, config_thp_doc.ini')

opt = parser.parse_args()
config = configparser.ConfigParser()
config.read(opt.config)
#导入参数ini

class DEMMAPSolverEnergyModel(AStar): #类继承
    """
    A simulation model that take the number of turns into consideration
    A note has three state, (x,y,lm(last move))
    Use 0,1,2,3,4,5,6,7 to represent right, right_up, up, up_left,
    left, left_down, down, down_right
    """
    def __init__(self, dem_map, lat, lon):
        self.dem_map = dem_map
        self.lat = lat # 纬度
        self.lon = lon # 经度
        self.width = dem_map.shape[1]
        self.height = dem_map.shape[0]

    def heuristic_cost_estimate(self, n1, n2): #？
        # take turns
        #(x1, y1, lm1) = n1
        #(x2, y2, lm2) = n2
        # do not take turns
        (x1, y1) = n1
        (x2, y2) = n2
        #manhanten
        return abs(x1-x2) + abs(y1-y2)
        #L_\{inf}
        #return max(abs(x1-x2), abs(y1-y2))

    def distance_between(self, n1, n2):
        # take turns
        #(x1, y1, lm1) = n1
        #(x2, y2, lm2) = n2
        # do not take tures
        (x1, y1) = n1
        (x2, y2) = n2
        if x1 == x2 and y1 == y2:
            return 0.0
        lat1 = self.lat[y1,x1]
        lat2 = self.lat[y2,x2]
        lon1 = self.lon[y1,x1]
        lon2 = self.lon[y2,x2]
        # take turns
        #if abs(lm1 - lm2) > 4:
            #angle = (8 - abs(lm1 - lm2)) * 3.14 / 4.0 # 1 corresponds to pi/4, 45 degree
        #else:
            #angle = abs(lm1 - lm2) * 3.14 / 4.0
        h = self.dem_map[y2, x2] - self.dem_map[y1, x1] #height difference of two road points
        d = compute_dis_lat_lon((lat1, lon1),(lat2, lon2)) #distance of two road points
        e = 0.0
        # horitical energy
        e += flight_energy(d,h)
        #e += height_energy(h)
        #e += height_energy2((x1, y1), (x2, y2), self.dem_map)
        # take turn energy
        #e += taketurn_energy(cita=angle)
        return e

    def neighbors(self, node):
        #x, y, lm = node
        #r = [(nx, ny, lm) for nx, ny, lm in[(x, y - 1, 2), (x, y + 1, 6), (x - 1, y, 4),
        #            (x + 1, y, 0), (x+1,y+1, 7), (x-1,y-1, 3),(x+1,y-1,1), (x-1,y+1,5)]
        #       if 0 <= nx < self.width and 0 <= ny < self.height]
        # do not take turns
        x, y = node
        return[(nx, ny) for nx, ny in[(x, y - 1), (x, y + 1), (x - 1, y), (x + 1, y), (x+1,y+1),
                                      (x-1,y-1), (x+1,y-1), (x-1,y+1)]
               if 0 <= nx < self.width and 0 <= ny < self.height]

    def checklineofsight(self, node1, node2): #bug判断航段上海拔是否存在高于航段两段最高海拔（对比运算修改，整数组对比，非最大值对比）
        alt_line = get_val_on_straight_line(self.dem_map, node1.data[0:2], node2.data[0:2])
        if alt_line.size == 2:
            return True
        else:
            #max_alt = np.max(alt_line[1:-1])
            #if max_alt > max((alt_line[0], alt_line[-1])): 
            hei_line=np.linspace(alt_line[0],alt_line[-1],np.size(alt_line))
            hei_line=np.round(hei_line).astype(np.int)
            for i in range(alt_line.size):
                if alt_line[i]>hei_line[i]+25:
                    return False
            #(x1, y1) = node1.data[0:2]
            #(x2, y2) = node2.data[0:2]
            #lat1 = self.lat[y1,x1]
            #lat2 = self.lat[y2,x2]
            #lon1 = self.lon[y1,x1]
            #lon2 = self.lon[y2,x2]
            #h = self.dem_map[y2, x2] - self.dem_map[y1, x1]
            #d = compute_dis_lat_lon((lat1, lon1),(lat2, lon2))
            #c=h*11/d
            #if c < -1.5:
            #    return False
            #else :#加上node1, node2，用这俩计算距离和高度差，计算坡度，判断大小返回值
            return True


# main
if __name__ == '__main__':
    lat, lon, alt, start_xy, stop_xy = parse_txt(opt.config)
    # take turns
    #start_xy = start_xy + (0,)
    #stop_xy = stop_xy + (0,)
    # do not take turns

    foundPath = list(DEMMAPSolverEnergyModel(alt, lat, lon).astar(start_xy, stop_xy)) #create the road point
    # show road points
    # do not take turn
  

  #for (x,y) in foundPath:
    #    alt[y,x] = np.max(alt)
    #plt.matshow(alt)
    #plt.show()



    # save road points
    save_file_name = 'road_point_%s_%s_%s.txt' % (config['craw_info']['from_'], config['craw_info']['to_'], config['craw_info']['surfix'])
    with open(save_file_name, 'w') as f:
        start_loc = eval(config['craw_info']['start_loc'])
        stop_loc = eval(config['craw_info']['stop_loc'])
        gap = eval(config['craw_info']['gap'])
        takeoff_v =  eval(config['craw_info']['takeoff_v'])
        route_v =  eval(config['craw_info']['route_v'])
        landing_v =  eval(config['craw_info']['landing_v']) 
        f.write('[{"lat":%f, "lng":%f, "alt":%f, "kml_alt":%f, "v":%f},\n' % ( start_loc[1], start_loc[0], 60, alt[start_xy[1],start_xy[0]]+60, takeoff_v))
        print('[{"lat":%f, "lng":%f, "alt":%f, "kml_alt":%f, "v":%f},' % (start_loc[1], start_loc[0], 60, alt[start_xy[1],start_xy[0]]+60, takeoff_v))
        for i in range(1, len(foundPath)-1):
            x, y = foundPath[i]
            f.write('{"lat":%f, "lng":%f, "alt":%f, "kml_alt":%f, "v":%f},\n' % (lon[y, x], lat[y, x], alt[y, x] - alt[start_xy[1],start_xy[0]] + gap, alt[y, x] + gap, route_v))
            #输出经纬度正确，高度正确，航线穿过地表，生成路点问题，路点在地表，航线全段未高过地表（路点前一段穿过地面）
            print('{"lat":%f, "lng":%f, "alt":%f, "kml_alt":%f, "v":%f},' % (lon[y, x], lat[y, x], alt[y, x] - alt[start_xy[1],start_xy[0]] + gap, alt[y, x] + gap, route_v))
        f.write('{"lat":%f, "lng":%f, "alt":%f, "kml_alt":%f, "v":%f}]\n' % (stop_loc[1], stop_loc[0], alt[stop_xy[1], stop_xy[0]] - alt[start_xy[1],start_xy[0]]+gap, alt[stop_xy[1], stop_xy[0]]+gap, landing_v))
        print('{"lat":%f, "lng":%f, "alt":%f, "kml_alt":%f, "v":%f}]' % (stop_loc[1], stop_loc[0], alt[stop_xy[1], stop_xy[0]] - alt[start_xy[1],start_xy[0]]+gap, alt[stop_xy[1], stop_xy[0]]+gap, landing_v))

        #pickle.dump(foundPath, open('path_%s_%s.pkl' % (cfg.from_, cfg.to_),'wb'))
